/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

/*      All code produced by the American Heritage School Robotics Team,
 *      The Ninjineers (#2383), for the 2013 FRC Season.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import tk.ninjineers2383.y2013.InterfaceDevices.ControlJoystick;
import tk.ninjineers2383.y2013.Components.Drivetrain;
import tk.ninjineers2383.y2013.Components.Shooter;
import tk.ninjineers2383.y2013.Components.Climber;
import tk.ninjineers2383.y2013.Components.Camera;

import edu.wpi.first.wpilibj.Timer;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    
    final int SHOOT_JOY_PORT = 2;
    final int DRIVE_JOY_PORT = 1;
    // final int STOP_BUTTON_PORT = 1;     //???
    
    //Define drive joystick buttons
    final int TURN = 1;
    final int CLIMB_UP = 3;
    final int CLIMB_DOWN = 2;
    final int HOOK_DEPLOY = 9;
    final int HOOK_WITHDRAW = 8;
    final int PIVOT_UP = 4;
    final int PIVOT_DOWN = 5;
    
    //Define shooter joystick buttons
    final int SPIN_UP = 5;
    final int SPIN_UP_15 = 9;
    final int SPIN_UP_20 = 8;
    //final int SPIN_UP_35 = 8;
    final int SPIN_UP_70 = 6;
    final int SPIN_UP_75 = 11;
    final int SPIN_UP_80 = 4;
    //final int SPIN_UP_85 = 3;
    //final int SPIN_UP_90 = 2;
    final int SPIN_UP_REVERSE = 7;
    final int LAUNCH = 1;
    final int LAUNCH_REVERSE = 10;
    final int SHOOTER_SPEED_UP = 3;
    final int SHOOTER_SPEED_DOWN = 2;
    
    //Define PWM channels for different components
    final int FR_DRIVE = 1;     //Victor
    final int FL_DRIVE = 2;     //Victor
    final int RR_DRIVE = 3;     //Victor
    final int RL_DRIVE = 4;     //Victor
    final int SHOOTER_MOTORS = 5;       //Victor
    final int SHOOTER_LAUNCH = 9;       //Victor
    final int CLIMB_MOTOR = 7;          //Victor
    final int SUPPORT_HOOKS = 8;        //Victor
    final int PIVOT_MOTOR = 6;          //Victor
    final int GYRO_CHANNEL = 2;         //Analog channel
    final int VEX_CHANNEL = 10;
    
    ControlJoystick ShootJoystick;
    ControlJoystick DriveJoystick;
    Drivetrain Drive;
    Shooter Shoot;
    Climber Climb;
    Camera Cam;
    Timer Time;
    
    double shooterSpeed = 0.8;
    boolean speedPressed = false;
    double distance;
    int camCount = 0;
    final double shooterSpeedIncrement = 0.05;
    final int REVERSE_STRAFE = 1;       // -1 to reverse strafe direction
    final int REVERSE_TURN = 1;         // -1 to reverse strafe direction
    final int LAUNCH_DIRECTION= 1;       // -1 to reverse launch direction
    final double DRIVE_X_THRES = 0.1;
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() 
    {
        ShootJoystick = new ControlJoystick(SHOOT_JOY_PORT);
        DriveJoystick = new ControlJoystick(DRIVE_JOY_PORT);
        Drive = new Drivetrain(FR_DRIVE, FL_DRIVE, RR_DRIVE, RL_DRIVE);
        Shoot = new Shooter(SHOOTER_MOTORS, SHOOTER_LAUNCH, PIVOT_MOTOR, GYRO_CHANNEL, VEX_CHANNEL);
        Climb = new Climber(CLIMB_MOTOR, SUPPORT_HOOKS);
        Cam = new Camera();
        Time = new Timer();
        
    }
    
    final double VEX_DELAY = 0.3;
    final double PIVOT_DELAY = 2.5;
    final double ACTUATOR_DELAY = 0.75;
    final double ACTUATOR_SPEED = 0.85;
    public void autonomousInit()
    {
        //autonomous1();
        Shoot.spinUp(-0.8);                             // Spin up launch motors
        Shoot.pivotDown();                              // Pivot down
        Time.delay(PIVOT_DELAY);
            Shoot.pivotStop();
        Time.delay(1.75);
            Shoot.launchGo(ACTUATOR_SPEED);             // Spin actuator
        Time.delay(ACTUATOR_DELAY);
            Shoot.launchStop();                         // Stop actuator
            Shoot.vexOut();                             // Vex in
        Time.delay(VEX_DELAY);
            Shoot.vexIn();                              // Vex out
        Time.delay(VEX_DELAY);
            Shoot.vexOut();
        Time.delay(0.01);
            Shoot.vexStop();
        Time.delay(1);                                  // Extra delay between shots
            Shoot.launchGo(ACTUATOR_SPEED);             // Spin actuator
        Time.delay(ACTUATOR_DELAY);
            Shoot.launchStop();                         // Stop actuator
            Shoot.vexOut();                             // Vex in
        Time.delay(VEX_DELAY);
            Shoot.vexIn();
        Time.delay(VEX_DELAY);
            Shoot.vexStop();
        Time.delay(1);                                  // Extra delay between shots
            Shoot.launchGo(ACTUATOR_SPEED);             // Spin actuator
        Time.delay(ACTUATOR_DELAY);
            Shoot.launchStop();
            Shoot.stop();
            Shoot.pivotUp();
        Time.delay(PIVOT_DELAY);
            Shoot.pivotStop();
        
        //Drive.setRight(0.5);
        //Drive.setLeft(0.5);
        Time.delay(0.5);
        Drive.setLeft(0);
        Drive.setRight(0);
        //setDisabled();        ??? HOW DO THIS ?!?!?
        Time.delay(20);
    }
    
    public void autonomous1()
    {
        Shoot.spinUp(0.8);  
        while (Math.abs(Shoot.getAngle()) < 30)
        {
            Shoot.pivotDown();
        }
        Shoot.pivotStop();
        Time.delay(2000);
        Shoot.launchGo(-1);
        Time.delay(5000);
        Shoot.launchStop();
        Shoot.stop();
        
        Drive.setRight(-0.5);
        Drive.setLeft(0.5);
        Time.delay(500);
        Drive.setLeft(0);
        Drive.setRight(0);
    }
    
    public void teleopContinous()
    {
        /*if (ShootJoystick.getThrottle() >= 0.9)
        {
             Drive.setRight(0);
             Drive.setLeft(0);
             Shoot.stop();
             Climb.stop();
             terminate();
        }*/
        
    }
    
    /*
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() 
    {
        // ******************** DRIVER JOYSTICK CONDITIONS ********************
        
        // Forward/Back drive check (stops if y-axis 0)
        if (DriveJoystick.getX() < DRIVE_X_THRES && DriveJoystick.getX() > -DRIVE_X_THRES && DriveJoystick.checkButton(TURN) == false)
        {
            Drive.setLeft(-1*DriveJoystick.getY());
            Drive.setRight(DriveJoystick.getY());
        }
        
        // strafe right check
        if (DriveJoystick.getX() > DRIVE_X_THRES && DriveJoystick.checkButton(TURN) == false)
        {
            Drive.strafe(REVERSE_STRAFE*DriveJoystick.getX());
        }
        // strafe left check
        if (DriveJoystick.getX() < -DRIVE_X_THRES && DriveJoystick.checkButton(TURN) == false)
        {
            Drive.strafe(REVERSE_STRAFE*DriveJoystick.getX());
        }
        
        // Turn right check
        if (DriveJoystick.checkButton(TURN) == true && DriveJoystick.getX() > DRIVE_X_THRES)
        {
            Drive.setLeft(REVERSE_TURN*DriveJoystick.getX());
            Drive.setRight(REVERSE_TURN*DriveJoystick.getX());
        }
        // Turn left check
        if (DriveJoystick.checkButton(TURN) == true && DriveJoystick.getX() < -DRIVE_X_THRES)
        {
            Drive.setLeft(REVERSE_TURN*DriveJoystick.getX());
            Drive.setRight(REVERSE_TURN*DriveJoystick.getX());
        }
        
        // Trigger press without direction: stop
        if (DriveJoystick.checkButton(TURN) == true && DriveJoystick.getX() > -DRIVE_X_THRES && DriveJoystick.getX() < DRIVE_X_THRES)
        {
            Drive.setLeft(0);
            Drive.setRight(0);
        }
        
        // Hook deploy check
        if (DriveJoystick.checkButton(HOOK_DEPLOY) == true && DriveJoystick.checkButton(HOOK_WITHDRAW) == false)
        {
            Climb.hookEngage();
        }
        // Hook withdraw check
        if (DriveJoystick.checkButton(HOOK_DEPLOY) == false && DriveJoystick.checkButton(HOOK_WITHDRAW) == true)
        {
            Climb.hookDisengage();
        }
        // Stop hooks if no button pressed
        if (DriveJoystick.checkButton(HOOK_DEPLOY) == false && DriveJoystick.checkButton(HOOK_WITHDRAW) == false)
        {
            Climb.hookStop();
        }
        // Stop hooks if both buttons pressed
        if (DriveJoystick.checkButton(HOOK_DEPLOY) == true && DriveJoystick.checkButton(HOOK_WITHDRAW) == true)
        {
            Climb.hookStop();
        }
        
        // Pivot up check
        if (DriveJoystick.checkButton(PIVOT_UP) == true && DriveJoystick.checkButton(PIVOT_DOWN) == false)
        {
            Shoot.pivotUp();
        }
        // Pivot down check
        if (DriveJoystick.checkButton(PIVOT_UP) == false && DriveJoystick.checkButton(PIVOT_DOWN) == true)
        {
            Shoot.pivotDown();
        }
        // Stop pivot if no button pressed
        if (DriveJoystick.checkButton(PIVOT_UP) == false && DriveJoystick.checkButton(PIVOT_DOWN) == false)
        {
            Shoot.pivotStop();
        }
        // Stop pivot if both buttons pressed
        if (DriveJoystick.checkButton(PIVOT_UP) == true && DriveJoystick.checkButton(PIVOT_DOWN) == true)
        {
            Shoot.pivotStop();
        }
        
        // Climb up check
        if (DriveJoystick.checkButton(CLIMB_UP) == true && DriveJoystick.checkButton(CLIMB_DOWN) == false)
        {
            Climb.ascend();
        }
        // Climb down check
        if (DriveJoystick.checkButton(CLIMB_UP) == false && DriveJoystick.checkButton(CLIMB_DOWN) == true)
        {
            Climb.descend();
        }
        // Stop climb if no button pressed
        if (DriveJoystick.checkButton(CLIMB_UP) == false && DriveJoystick.checkButton(CLIMB_DOWN) == false)
        {
            Climb.stop();
        }
        // Stop climb if both buttons pressed
        if (DriveJoystick.checkButton(CLIMB_UP) == true && DriveJoystick.checkButton(CLIMB_DOWN) == true)
        {
            Climb.stop();
        }
        
        // ******************** SHOOTER JOYSTICK CONDITIONS ********************
        
        // Spin-up checks
        if (ShootJoystick.checkButton(SPIN_UP) == true)
        {
            Shoot.spinUp(-shooterSpeed);
        }
        if (ShootJoystick.checkButton(SPIN_UP_15) == true)
        {
            shooterSpeed = 0.15;
            Shoot.spinUp(-shooterSpeed);
        }
        if (ShootJoystick.checkButton(SPIN_UP_20) == true)
        {
            shooterSpeed = 0.20;
            Shoot.spinUp(-shooterSpeed);
        }
        /*if (ShootJoystick.checkButton(SPIN_UP_35) == true)
        {
            shooterSpeed = 0.35;
            Shoot.spinUp(-shooterSpeed);
        }*/
        if (ShootJoystick.checkButton(SPIN_UP_70) == true)
        {
            shooterSpeed = 0.70;
            Shoot.spinUp(-shooterSpeed);
        }
        if (ShootJoystick.checkButton(SPIN_UP_75) == true)
        {
            shooterSpeed = 0.75;
            Shoot.spinUp(-shooterSpeed);
        }
        if (ShootJoystick.checkButton(SPIN_UP_80) == true)
        {
            shooterSpeed = 0.80;
            Shoot.spinUp(-shooterSpeed);
        }
        /*if (ShootJoystick.checkButton(SPIN_UP_85) == true)
        {
            shooterSpeed = 0.85;
            Shoot.spinUp(-shooterSpeed);
        }
        if (ShootJoystick.checkButton(SPIN_UP_90) == true)
        {
            shooterSpeed = 0.90;
            Shoot.spinUp(-shooterSpeed);
        }*/
        
        // Spin-up stop check
        if (ShootJoystick.checkButton(SPIN_UP_REVERSE) == false && 
                ShootJoystick.checkButton(SPIN_UP_15) == false && 
                ShootJoystick.checkButton(SPIN_UP_20) == false && 
                ShootJoystick.checkButton(SPIN_UP) == false && 
                ShootJoystick.checkButton(SPIN_UP_70) == false && 
                ShootJoystick.checkButton(SPIN_UP_75) == false && 
                ShootJoystick.checkButton(SPIN_UP_80) == false)
        {
            Shoot.spinUp(0);
        }
        
        // Spin-up reverse check
        if (ShootJoystick.checkButton(SPIN_UP_REVERSE) == true)
        {
            Shoot.spinUp(0.90);
        }
        // Stop spin-up if both buttons pressed
        //if (ShootJoystick.checkButton(SPIN_UP) == true && ShootJoystick.checkButton(SPIN_UP_REVERSE) == true)
        //{
        //    Shoot.spinUp(0);
        //}
        
        // Spin-up speed increase check
        if (ShootJoystick.checkButton(SHOOTER_SPEED_UP) == true && shooterSpeed <= 1.0 && speedPressed == false)
        {
            shooterSpeed += shooterSpeedIncrement;
            speedPressed = true;
        }
        // Spin-up speed decrease check
        if (ShootJoystick.checkButton(SHOOTER_SPEED_DOWN) == true && shooterSpeed >= 0.05 && speedPressed == false)
        {
            shooterSpeed -= shooterSpeedIncrement;
            speedPressed = true;
        }
        // Spin-up speed button check
        if (ShootJoystick.checkButton(SHOOTER_SPEED_DOWN) == false && ShootJoystick.checkButton(SHOOTER_SPEED_UP) == false)
        {
            speedPressed = false;
        }
        
        // Launcher check
        if (ShootJoystick.checkButton(LAUNCH) == true && ShootJoystick.checkButton(LAUNCH_REVERSE) == false)
        {
            Shoot.launchGo(LAUNCH_DIRECTION);
        }
        // Launcher reverse check
        if (ShootJoystick.checkButton(LAUNCH) == false && ShootJoystick.checkButton(LAUNCH_REVERSE) == true)
        {
            Shoot.launchGo(-1*LAUNCH_DIRECTION);
        }
        // Stop launcher if no button pressed
        if (ShootJoystick.checkButton(LAUNCH) == false && ShootJoystick.checkButton(LAUNCH_REVERSE) == false)
        {
            Shoot.launchStop();
        }
        // Stop launcher if both buttons pressed
        if (ShootJoystick.checkButton(LAUNCH) == true && ShootJoystick.checkButton(LAUNCH_REVERSE) == true)
        {
            Shoot.launchStop();
        }
        
        // Check vex conditions
        if (ShootJoystick.getY()>= 0.2)
        {
            Shoot.vexIn();
        }
        else if (ShootJoystick.getY()<= -0.2)
        {
            Shoot.vexOut();
        }
        else
        {
            Shoot.vexStop();
        }
        
        /*
        if (camCount % 50 == 0)
        {
            distance = Cam.run();
        }
        camCount++;
        */
        SmartDashboard.putNumber("Shooter Speed", shooterSpeed*100);
        //SmartDashboard.putNumber("Shooter Angle", Shoot.getAngle());
        //SmartDashboard.putNumber("Distance", distance);
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() 
    {
       
    }
    
    
}